#include <iostream>
#include "Frame.h"
#include "Map.h"
#include "KeyFrame.h"
#include "MapPoint.h"
#include <opencv2/core/core.hpp>
#include <chrono>
#include <thread>


#include "g2o/g2o/core/block_solver.h"
#include "g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/g2o/solvers/linear_solver_eigen.h"
#include "g2o/g2o/types/types_six_dof_expmap.h"
#include "g2o/g2o/core/robust_kernel_impl.h"
#include "g2o/g2o/solvers/linear_solver_dense.h"
#include "g2o/g2o/types/types_seven_dof_expmap.h"

#include "Converter.h"
#include <stdint.h>
#include "Tracking.h"
#include "LocalMapping.h"


#if 1
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC_TimeStamps/V201.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/cam1/data"
#else
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC_TimeStamps/V203.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/cam1/data"
#endif

using namespace std;
using namespace cv;

void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps);



int main(int argc, char **argv)
{

	cout << "hello world" << endl;

	vector<string> vstrImageLeft;
	vector<string> vstrImageRight;
	vector<double> vTimeStamp;

	LoadImages(EUROC_DATA_LEFT_FILE_PATH, EUROC_DATA_RIGHT_FILE_PATH, IMAGE_TIMESTAMP_FILE_PATH, 
				vstrImageLeft, vstrImageRight, vTimeStamp);

	 if(vstrImageLeft.empty() || vstrImageRight.empty())
	 {
		cout << "load images failed" << endl;
		return -1;
	 }

	 if(vstrImageLeft.size()!=vstrImageRight.size())
	 {
		cout << "left image size: " << vstrImageLeft.size() <<" is not equal right image size: " << vstrImageRight.size() << endl;
		return -1;
	 }

	cv::FileStorage fsSettings(CAMERA_PARAMTERS_FILE_PATH, cv::FileStorage::READ);
	if(!fsSettings.isOpened())
	{
		cout << "open camera paramters file failed :" << CAMERA_PARAMTERS_FILE_PATH << endl;
		return -1;
	}

	cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

	if(K_l.empty() || K_r.empty() ||
		P_l.empty() || P_r.empty() ||
		R_l.empty() || R_r.empty() ||
		D_l.empty() || D_r.empty() ||
		rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
	{
		cout << "Calibration parameters to rectify stereo are missing!" << endl;
	}


	cv::Mat mK;
    cv::Mat mDistCoef;
    float fx = fsSettings["Camera.fx"];
    float fy = fsSettings["Camera.fy"];
    float cx = fsSettings["Camera.cx"];
    float cy = fsSettings["Camera.cy"];

    cv::Mat K = cv::Mat::eye(3,3,CV_32F);
    K.at<float>(0,0) = fx;
    K.at<float>(1,1) = fy;
    K.at<float>(0,2) = cx;
    K.at<float>(1,2) = cy;
    K.copyTo(mK);

    cv::Mat DistCoef(4,1,CV_32F);
    DistCoef.at<float>(0) = fsSettings["Camera.k1"];
    DistCoef.at<float>(1) = fsSettings["Camera.k2"];
    DistCoef.at<float>(2) = fsSettings["Camera.p1"];
    DistCoef.at<float>(3) = fsSettings["Camera.p2"];
    const float k3 = fsSettings["Camera.k3"];
    if(k3!=0)
    {
        DistCoef.resize(5);
        DistCoef.at<float>(4) = k3;
    }
    DistCoef.copyTo(mDistCoef);

    float mbf = fsSettings["Camera.bf"];

    float fps = fsSettings["Camera.fps"];

	cout << "mb:" << mbf / fx << ", fps:" << fps << endl;

	float mThDepth;
	mThDepth = mbf * (float)fsSettings["ThDepth"] / fx;

	cout << "mThDepth :" << mThDepth << endl;

	float mKeyFrameSize = fsSettings["Viewer.KeyFrameSize"];
    float mKeyFrameLineWidth = fsSettings["Viewer.KeyFrameLineWidth"];
    float mGraphLineWidth = fsSettings["Viewer.GraphLineWidth"];
    float mPointSize = fsSettings["Viewer.PointSize"];
    float mCameraSize = fsSettings["Viewer.CameraSize"];
    float mCameraLineWidth = fsSettings["Viewer.CameraLineWidth"];


	float mViewpointX = fsSettings["Viewer.ViewpointX"];
    float mViewpointY = fsSettings["Viewer.ViewpointY"];
    float mViewpointZ = fsSettings["Viewer.ViewpointZ"];
    float mViewpointF = fsSettings["Viewer.ViewpointF"];



	cv::Mat M1l,M2l,M1r,M2r;
    cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
    cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);

	cv::Mat imLeft, imRight, imLeftRect, imRightRect;

	Tracking* mpTracker;
	LocalMapping* mpLocalMapper;
	MapDrawer* mpMapDrawer;
	Viewer* mpViewer;

	std::thread* mptLocalMapping;
	std::thread* mptViewer;


	Map* mpMap;
	//Create the Map
	mpMap = new Map();

	mpMapDrawer = new MapDrawer(mpMap, mKeyFrameSize, mKeyFrameLineWidth, mGraphLineWidth, mPointSize, mCameraSize, mCameraLineWidth);

	mpTracker = new Tracking(mpMap, mpMapDrawer);

	mpLocalMapper = new LocalMapping(mpMap);
	mptLocalMapping = new thread(&LocalMapping::Run, mpLocalMapper);

	mpViewer = new Viewer(mpMapDrawer, mViewpointX, mViewpointY, mViewpointZ, mViewpointF, fps);
    mptViewer = new thread(&Viewer::Run, mpViewer);
	mpTracker->SetViewer(mpViewer);


	mpTracker->SetLocalMapper(mpLocalMapper);

	for(int i=0; i<vstrImageLeft.size(); i++)
	{
		// Read left and right images from file
        imLeft = cv::imread(vstrImageLeft[i],IMREAD_GRAYSCALE);
        imRight = cv::imread(vstrImageRight[i],IMREAD_GRAYSCALE);

		 if(imLeft.empty() || imRight.empty())
		 {
			cout << "read image faield" << endl;
			return -1;
		 }

		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

		cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
		cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

		//--------------------------------------------------------------

		mpTracker->track(imLeftRect, imRightRect,vTimeStamp[i], mK, mDistCoef, mbf, mThDepth);

		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
		double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
		//cout << "ttrack:" << ttrack * 1e3 << " ms" << endl;
	}

	printf("game over\n");

	mpViewer->RequestFinish();
	mpLocalMapper->RequestFinish();
	mptLocalMapping->join();
	mptViewer->join();

	return 0;
}

void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps)
{
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImageLeft.reserve(5000);
    vstrImageRight.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png");
            vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);

        }
    }
}


